#ifndef I_LIB_PC_I_SEGM_H
#define I_LIB_PC_I_SEGM_H

#include <vector>
#include <cassert>
#include <ctime>
#include <cstdio>
#include <iostream>
#include <algorithm>

#include "../core/i_rand.h"
#include "../core/i_struct.h"
#include "../ip/i_pyramid.h"
#include "../ip/i_nms.h"
#include "../ip/i_morphology.h"

namespace idl
{
    template<typename T>
    class OCC
    {
    public:
        OCC(){};
        ~OCC(){};
        OCC(const OCC<T>& occ)
        {
            if (!occ.empty())
            {
                _xs.resize(occ.size());
                _ys.resize(occ.size());
                i_copy(occ.const_data_x(), _xs.data(), (int)(occ.size()));
                i_copy(occ.const_data_y(), _ys.data(), (int)(occ.size()));
            }
        }
        OCC& operator=(const OCC<T>& occ)
        {
            if (this != &occ)
            {
                this->_xs.resize(occ.size());
                this->_ys.resize(occ.size());
                i_copy(occ.const_data_x(), this->_xs.data(), (int)(occ.size()));
                i_copy(occ.const_data_y(), this->_ys.data(), (int)(occ.size()));
            }
            return (*this);
        }
        void clear()
        {
            _xs.clear();
            _ys.clear();
        }
        bool empty() const
        {
            assert(_xs.size() == _ys.size());
            return (_xs.empty());
        }
        unsigned int size() const 
        { 
            assert(_xs.size() == _ys.size());
            return _xs.size(); 
        }
        const T* const_data_x() const
        {
            return _xs.data();
        }
        const T* const_data_y() const
        {
            return _ys.data();
        }
        const T x(int i) const
        {
            assert(i >= 0 && i < (int)_xs.size());
            return _xs[i];
        }
        const T y(int i) const
        {
            assert(i >= 0 && i < (int)_ys.size());
            return _ys[i];
        }
        const T x(unsigned int i) const
        {
            assert(i < _xs.size());
            return _xs[i];
        }
        const T y(unsigned int i) const
        {
            assert(i < _ys.size());
            return _ys[i];
        }
        void push_back(T x, T y)
        {
            _xs.push_back(x);
            _ys.push_back(y);
        }
        T get_centroid_x() const
        {
            T cen = (T)0.0;
            unsigned int i, size = _xs.size();
            for (i = 0; i < size; ++i)
            {
                cen += _xs[i];
            }
            if (size)
            {
                cen = cen / size;
            }
            return cen;
        }
        T get_centroid_y() const
        {
            T cen = (T)0.0;
            unsigned int i, size = _ys.size();
            for (i = 0; i < size; ++i)
            {
                cen += _ys[i];
            }
            if (size)
            {
                cen = cen / size;
            }
            return cen;
        }
    private:
        std::vector<T> _xs;
        std::vector<T> _ys;
    };

    /*operator+(,), note this function does not check duplicated elements*/
    template <typename T>
    inline OCC<T> operator+(const OCC<T>& a, const OCC<T>& b) 
    {
        OCC<T> result(a);
        unsigned int i;
        for (i = 0; i < b.size(); ++i)
        {
            result.push_back(b.x(i), b.y(i));
        }
        return result;
    }

    /*operator+=(,) note this function does not check duplicated elements*/
    template <typename T>
    inline OCC<T>& operator+=(OCC<T>& a, const OCC<T>& b)
    {
        unsigned int i;
        for (i = 0; i < b.size(); ++i)
        {
            a.push_back(b.x(i), b.y(i));
        }
        return a;
    }

    /*Strictly smaller*/
    template<typename T>
    inline bool i_occ_smaller(const OCC<T>& a, const OCC<T>& b)
    {
        return a.size() < b.size();
    }

    /*Strictly larger*/
    template<typename T>
    inline bool i_occ_larger(const OCC<T>& a, const OCC<T>& b)
    {
        return a.size() > b.size();
    }
    
    template<typename T>
    class OCCArray
    {
    public:
        OCCArray(){};
        ~OCCArray(){};
        OCCArray& operator=(const OCCArray<T>& a)
        {
            if (this != &a)
            {
                this->_array.resize(a.size());
                for (unsigned int i = 0; i < a.size(); ++i)
                {
                    _array[i] = a[i];
                }
            }
            return (*this);
        };
    
        unsigned int size() const
        {
            return _array.size();
        }

        int init(const unsigned int* const*labels, int width, int height);

        void cleanup();

        void clear()
        {
            _array.clear();
        }
        void resize(int n)
        {
            _array.clear();
            _array.resize(n);
            cleanup();
        }
        void resize(unsigned int n)
        {
            _array.clear();
            _array.resize(n);
            cleanup();
        }
        void push_back(const OCC<T>& cc)
        {
            if (!cc.empty())
            {
                _array.push_back(cc);
            }
        }

        OCC<T>& operator[](unsigned int i)
        {
            assert(i >= 0 && i < _array.size());
            return _array[i];
        }

        const OCC<T>& operator[](unsigned int i) const
        {
            assert(i >= 0 && i < _array.size());
            return _array[i];
        }
        
        OCC<T>& operator[](int i)
        {
            assert(i >= 0 && i < (int)_array.size());
            return _array[i];
        }
        
        const OCC<T>& operator[](int i) const
        {
            assert(i >= 0 && i < (int)_array.size());
            return _array[i];
        }

        const OCC<T>* cbegin() const
        {
            return _array.data();
        }
        
        const OCC<T>* const cend() const
        {
            return _array.data() + _array.size();
        }
        
        OCC<T>* begin()
        {
            return _array.data();
        }

        OCC<T>* end()
        {
            return _array.data() + _array.size();
        }

    private:
        std::vector<OCC<T> > _array;
    };

    template<typename T>
    int OCCArray<T>::init(const unsigned int* const*labels, 
                          int width, 
                          int height)
    {
        int x, y;
        unsigned int l; 
        unsigned int nr_cc = i_max_element(labels[0], width*height);
        
        _array.clear();
        
        if (!nr_cc)
        {
            return 0;
        }
        
        _array.resize(nr_cc);

        for (y = 0; y < height; ++y)
        {
            for (x = 0; x < width; ++x)
            {
                l = labels[y][x];
                if (l > 0)
                {
                    _array[l - 1].push_back((T)x, (T)y);
                }
            }
        }

        return (int)nr_cc;
    }

    template<typename T>
    void OCCArray<T>::cleanup()
    {
        for (unsigned int i = 0; i < _array.size(); ++i)
        {
            _array[i].clear();
        }
    }

    template<typename T>
    class OCCTable
    {
    public:
        OCCTable(){};
        ~OCCTable(){};
        unsigned int size() const
        {
            return _table.size();
        }
        void clear()
        {
            _table.clear();
        }
        void resize(int n)
        {
            _table.clear();
            _table.resize(n);
        }
        void push_back(const OCCArray<T>& a)
        {
            _table.push_back(a);
        }
        OCCArray<T>& operator[](unsigned int i)
        {
            assert(i < _table.size());
            return _table[i];
        }
        const OCCArray<T>& operator[](unsigned int i) const
        {
            assert(i < _table.size());
            return _table[i];
        }
        OCCArray<T>& operator[](int i)
        {
            assert(i >= 0 && i < (int)_table.size());
            return _table[i];
        }
        const OCCArray<T>& operator[](int i) const
        {
            assert(i >= 0 && i < (int)_table.size());
            return _table[i];
        }
        OCC<T>& operator()(unsigned int r, unsigned int c)
        {
            assert(r < _table.size());
            assert(c < _table[r].size());
            return _table[r][c];
        }
        const OCC<T>& operator()(unsigned int r, unsigned int c) const
        {
            assert(r < _table.size());
            assert(c < _table[r].size());
            return _table[r][c];
        }
        OCC<T>& operator()(int r, int c)
        {
            assert(r >= 0 && r < _table.size());
            assert(c >= 0 && c < _table[r].size());
            return _table[r][c];
        }
        const OCC<T>& operator()(int r, int c) const
        {
            assert(r >= 0 && r < _table.size());
            assert(c >= 0 && c < _table[r].size());
            return _table[r][c];
        }
    private:
        std::vector<OCCArray<T> > _table;
    };

    template<typename T>
    class OCCDetector
    {
    public:
        OCCDetector();
        ~OCCDetector();
        bool init(int width, int height, int scale);
        bool initialized() const 
        { 
            return _initialized;
        };

        int detect(const float * const *prob_map,
            const float *offset_map,
            unsigned short *labels,
            unsigned char *canvas,
            float prob_cutoff_thre_contribute,
            float prob_cutoff_thre_seed,
            float prob_cutoff_thre_low,
            float prob_cutoff_thre_high,
            double cc_to_center_dist_thre,
            bool verbose = true);

        const unsigned int* get_labels(unsigned int scale = 0) const
        {
            if (!_initialized)
            {
                return NULL;
            }
            return (_labels->const_data(scale));
        }

        const float* get_probabilities(unsigned int scale = 0) const
        {
            if (!_initialized)
            {
                return NULL;
            }
            return (_probs->const_data(scale));
        }

        const unsigned char* get_masks(unsigned int scale = 0) const
        {
            if (!_initialized)
            {
                return NULL;
            }
            return (_masks->const_data(scale));
        }

        int nr_scale() const 
        { 
            if (!_initialized)
            {
                return 0;
            }
            return (int)(_occs->size());
        };

        int width() const
        {
            if (!_initialized)
            {
                return 0;
            }
            return _labels->width();
        }

        int height() const
        {
            if (!_initialized)
            {
                return 0;
            }
            return _labels->height();
        }

        int width(unsigned int scale) const
        {
            if (!_initialized)
            {
                return 0;
            }
            return _labels->width(scale);
        }

        int height(unsigned int scale) const
        {
            if (!_initialized)
            {
                return 0;
            }
            return _labels->height(scale);
        }

    private:
        void cleanup();

    private:
        MultiScaleLumPyramid<float>         *_probs;  /*masks*/
        MultiScaleLumPyramid<unsigned char> *_masks;  /*masks*/
        MultiScaleLumPyramid<unsigned int>  *_labels; /*labels*/
        OCCTable<T>                         *_occs;   /*multi-scale cc array*/
        unsigned int  **_workarray_uint;
        unsigned char **_workarray_uchar;
        float **_workarray_flt, **_workarray_flt_up;
        float **_upsample_seed_map;
        bool _initialized;
    };

    template <typename T>
    OCCDetector<T>::OCCDetector() :
        _initialized(false), _probs(NULL), _masks(NULL), 
        _labels(NULL), _occs(NULL), 
        _workarray_uint(NULL),
        _workarray_uchar(NULL),
        _workarray_flt(NULL),
        _workarray_flt_up(NULL),
        _upsample_seed_map(NULL){}

    template <typename T>
    OCCDetector<T>::~OCCDetector()
    {
        cleanup();
    }

    template <typename T>
    void OCCDetector<T>::cleanup()
    {
        if (_probs)
        {
            delete _probs;
        }
        if (_masks)
        {
            delete _masks;
        }
        if (_labels)
        {
            delete _labels;
        }
        if (_occs)
        {
            delete _occs;
        }
        if (_workarray_uint)
        {
            i_free2<unsigned int>(_workarray_uint);
        }
        if (_workarray_uchar)
        {
            i_free2<unsigned char>(_workarray_uchar);
        }
        if (_workarray_flt)
        {
            i_free2<float>(_workarray_flt);
        }
        if (_workarray_flt_up)
        {
            i_free2<float>(_workarray_flt_up);
        }
        if (_upsample_seed_map)
        {
            i_free2<float>(_upsample_seed_map);
        }
        _initialized = false;
    }

    template <typename T>
    bool OCCDetector<T>::init(int width, int height, int scale)
    {
        if (_initialized)
        {
            cleanup();
        }

        _probs = new MultiScaleLumPyramid<float>();
        _masks = new MultiScaleLumPyramid<unsigned char>();
        _labels = new MultiScaleLumPyramid<unsigned int>();
        _occs   = new OCCTable<T>();
        _workarray_uint = i_alloc2<unsigned int>(height, width);
        _workarray_uchar = i_alloc2<unsigned char>(height, width);
        _workarray_flt = i_alloc2<float>(height, width);
        _workarray_flt_up = i_alloc2<float>((height << 1), (width << 1));
        _upsample_seed_map = i_alloc2<float>((height << 1), (width << 1));

        if (!_probs || !_masks || !_labels || !_occs || !_workarray_uint || !_workarray_uchar || 
            !_workarray_flt || !_workarray_flt_up || !_upsample_seed_map)
        {
            cleanup();
            return false;
        }

        _probs->init(width, height, scale);
        _masks->init(width, height, scale);
        _labels->init(width, height, scale);
        _occs->resize(scale);

        if (!_probs->initialized() || !_masks->initialized() || !_labels->initialized())
        {
            cleanup();
            return false;
        }

        _initialized = true;
        return (true);
    }

    template<typename T>
    T point_to_cc_distance(const Pair<T, T> &pt, /*seed position <x, y>*/
        const OCC<T>& cc,
        const float* offset,
        unsigned int width,
        unsigned int height)
    {
        if (cc.size() == 0)
        {
            return Constant<T>::MAX_VAL();
        }

        T dist_to_pt = (T)0.0;
        int x, y, count = 0;
        float dx, dy;
        T cx, cy;
        T px = pt.first;
        T py = pt.second;

        unsigned int i, j, size_cc = cc.size();

        for (i = 0; i < size_cc; ++i)
        {
            x = i_round(cc.x(i));
            y = i_round(cc.y(i));

            if (x >= (int)width  || 
                y >= (int)height || 
                x < 0 || y < 0)
            {
                continue;
            }

            j = ((y * width + x) << 1);

            dy = offset[j];
            dx = offset[j + 1];

            cx = (T)x + (T)dx;
            cy = (T)y + (T)dy;

            dist_to_pt += i_sqrt(i_sqr(px - cx) + i_sqr(py - cy));
            count++;
        }

        if (!count)
        {
            return Constant<T>::MAX_VAL();
        }

        return i_div(dist_to_pt, count);
    }

    template<typename T>
    T point_to_seed_distance(const Pair<T, T> &pt, /*seed position <x, y>*/
        unsigned int x,
        unsigned int y,
        const float* offset,
        unsigned int width,
        unsigned int height)
    {
        T dist_to_pt = (T)0.0;
        float dx, dy;
        T cx, cy;
        T px = pt.first;
        T py = pt.second;

        if (x >= width || y >= height)
        {
            return Constant<T>::MAX_VAL();
        }

        unsigned int i = ((y * width + x) << 1);

        dy = offset[i];
        dx = offset[i + 1];

        cx = (T)x + (T)dx;
        cy = (T)y + (T)dy;

        dist_to_pt = i_sqrt(i_sqr(px - cx) + i_sqr(py - cy));
        
        return dist_to_pt;
    }

    template<typename T>
    T neighbor_to_seed_distance(const Pair<T, T> &pt, /*seed position <x, y>*/
        unsigned int x,
        unsigned int y,
        const float* offset,
        unsigned int width,
        unsigned int height)
    {
        unsigned int i, j;
        T dist_to_pt = (T)0.0;
        float dx, dy;
        T cx, cy;
        T px = pt.first;
        T py = pt.second;

        if (x >= width || y >= height)
        {
            return Constant<T>::MAX_VAL();
        }

        /*boundary case:*/
        if (x == 0 || y == 0 || x == width - 1 || y == height - 1)
        {
            return point_to_seed_distance(pt, x, y, offset, width, height);
        }

        /*center:*/
        i = (y * width + x);
        j = (i << 1);
            
        dy = offset[j];
        dx = offset[j + 1];

        cx = (T)x + (T)dx;
        cy = (T)y + (T)dy;

        dist_to_pt = i_sqrt(i_sqr(px - cx) + i_sqr(py - cy)) * 4;

        /*left:*/
        i = (y * width + x - 1);
        j = (i << 1);

        dy = offset[j];
        dx = offset[j + 1];

        cx = (T)(x - 1) + (T)dx;
        cy = (T)y + (T)dy;

        dist_to_pt += i_sqrt(i_sqr(px - cx) + i_sqr(py - cy));

        /*right:*/
        i = (y * width + x + 1);
        j = (i << 1);

        dy = offset[j];
        dx = offset[j + 1];

        cx = (T)(x + 1) + (T)dx;
        cy = (T)y + (T)dy;

        dist_to_pt += i_sqrt(i_sqr(px - cx) + i_sqr(py - cy));

        /*up:*/
        i = ((y - 1) * width + x);
        j = (i << 1);

        dy = offset[j];
        dx = offset[j + 1];

        cx = (T)x + (T)dx;
        cy = (T)(y-1) + (T)dy;

        dist_to_pt += i_sqrt(i_sqr(px - cx) + i_sqr(py - cy));

        /*down:*/
        i = ((y + 1) * width + x);
        j = (i << 1);

        dy = offset[j];
        dx = offset[j + 1];

        cx = (T)x + (T)dx;
        cy = (T)(y + 1) + (T)dy;

        dist_to_pt += i_sqrt(i_sqr(px - cx) + i_sqr(py - cy));
        
        /*normalize:*/
        return (dist_to_pt / 8);
    }
    
    /*inplace update output*/
    template <typename T>
    int match_node_and_seeds(const unsigned char *const *mask,
        const float *offset_map,
        const std::vector<Pair<T, T> >& seeds,
        OCCArray<T>& output,
        unsigned int width,
        unsigned int height,
        double max_dist_thre)
    {
        unsigned int i, j, k, nr_matched_node = 0;
        unsigned int nr_seeds = seeds.size();
        int best;
        /*note that the output vector will be updated incrementally, so unless
        the size is not consistent, it will not be cleared at the beginning of this routine*/
        if (output.size() != nr_seeds)
        {
            output.resize(nr_seeds);
        }

        if (!nr_seeds || !mask)
        {
            return (0);
        }

        T dist, best_dist;

        for (i = 0; i < height; ++i)
        {
            for (j = 0; j < width; ++j)
            {
                if (!mask[i][j])
                {
                    continue;
                }
                
                best = 0;
                best_dist = point_to_seed_distance<T>(seeds[0], j, i, offset_map, width, height);

                for (k = 1; k < nr_seeds; ++k)
                {
                    dist = point_to_seed_distance<T>(seeds[k], j, i, offset_map, width, height);
                    
                    if (dist < best_dist)
                    {
                        best = (int)k;
                        best_dist = dist;
                    }
                }
                /*no seed can be matched*/
                if ((double)best_dist >= max_dist_thre)
                {
                    continue;
                }
                else
                {
                    output[best].push_back((T)j, (T)i);
                    nr_matched_node++;
                }
            }
        }
        return (int)(nr_matched_node);
    }

    /*inplace update output*/
    template <typename T>
    int match_ccs_and_seeds(const OCCArray<T>& occs,
                            const float *offset_map,
                            const std::vector<Pair<T, T> >& seeds,
                            OCCArray<T>& output,
                            unsigned int width,
                            unsigned int height,
                            double max_dist_thre,
                            unsigned int min_size_thre)
    {
        unsigned int i, j, best, nr_matched_cc = 0;
        unsigned int nr_seeds = seeds.size();
        unsigned int nr_occs = occs.size();
        /*note that the output vector will be updated incrementally, so unless
        the size is not consistent, it will not be cleared at the beginning of this routine*/
        if (output.size() != nr_seeds)
        {
            output.resize(nr_seeds);
        }

        if (!nr_seeds || !nr_occs)
        {
            return (0);
        }
       
        T dist, best_dist;

        for (i = 0; i < nr_occs; ++i)
        {
            const OCC<T>& cc = occs[i];

            /*including empty case*/
            if (cc.size() <= min_size_thre) 
            {
                continue;
            }

            best = 0;
            best_dist = point_to_cc_distance<T>(seeds[0], cc, offset_map, width, height);

            for (j = 1; j < nr_seeds; ++j)
            {
                dist = point_to_cc_distance<T>(seeds[j], cc, offset_map, width, height);
                if (dist < best_dist)
                {
                    best = j;
                    best_dist = dist;
                }
            }

            /*no seed can be matched*/
            if ((double)best_dist >= max_dist_thre)
            {
                continue; 
            }
            else
            {
                /*push back*/
                output[best] += cc;
                nr_matched_cc++;
            }
        }

        /*note that output may contain empty entry*/
        return (int)(nr_matched_cc);
    }

    /*assume boundary condition is safe: r, c are within [1 - width(height)-1)*/
    inline unsigned int query_neighbor_cc(const unsigned int *const*label, int r, int c)
    {
        unsigned int l = 0;
        int count = 0;
        int rm1 = r - 1;
        int rp1 = r + 1;
        int cm1 = c - 1;
        int cp1 = c + 1;

        if (label[rm1][cm1])
        {
            l = label[rm1][cm1];
            count++;
        }
        if (label[rm1][c])
        {
            if (l != label[rm1][c])
            {
                count++;
                l = label[rm1][c];
            }
            if (count > 1)
            {
                return (0);
            }
        }
        if (label[rm1][cp1])
        {
            if (l != label[rm1][cp1])
            {
                count++;
                l = label[rm1][cp1];
            }
            if (count > 1)
            {
                return (0);
            }
        }
        if (label[r][cm1])
        {
            if (l != label[r][cm1])
            {
                count++;
                l = label[r][cm1];
            }
            if (count > 1)
            {
                return (0);
            }
        }
        if (label[r][cp1])
        {
            if (l != label[r][cp1])
            {
                count++;
                l = label[r][cp1];
            }
            if (count > 1)
            {
                return (0);
            }
        }
        if (label[rp1][cm1])
        {
            if (l != label[rp1][cm1])
            {
                count++;
                l = label[rp1][cm1];
            }
            if (count > 1)
            {
                return (0);
            }
        }
        if (label[rp1][c])
        {
            if (l != label[rp1][c])
            {
                count++;
                l = label[rp1][c];
            }
            if (count > 1)
            {
                return (0);
            }
        }
        if (label[rp1][cp1])
        {
            if (l != label[rp1][cp1])
            {
                count++;
                l = label[rp1][cp1];
            }
            if (count > 1)
            {
                return (0);
            }
        }
        return (count == 1) ? l : 0;
    }

    template <typename T>
    void propagate_neighbor_cc(const unsigned int *const*labels,
                               OCCArray<T>& ccs,
                               unsigned char **mask,
                               int width,
                               int height)
    {
        int r, c;
        unsigned int l;
        for (r = 1; r < height-1; ++r)
        {
            for (c = 1; c < width-1; ++c)
            {
                if (!mask[r][c])
                {
                    continue;
                }
                l = query_neighbor_cc(labels, r, c);
                if (l > 0)
                {
                    /*cc id*/
                    l = l - 1;
                    assert(l < ccs.size());
                    ccs[l].push_back((T)c, (T)r);
                    mask[r][c] = 0;
                }
            }
        }
    }

    template <typename T>
    int OCCDetector<T>::detect(const float * const *prob_map,
        const float *offset_map,
        unsigned short *labels,
        unsigned char *canvas,
        float prob_cutoff_thre_contribute,
        float prob_cutoff_thre_seed,
        float prob_cutoff_thre_low,
        float prob_cutoff_thre_high,
        double cc_to_center_dist_thre,
        bool verbose)
    {
        if (!offset_map || !prob_map)
        {
            return (-2);
        }

        if (!_initialized)
        {
            return (-1);
        }

        const unsigned int nr_iter = 2;
        const int width = _labels->width();
        const int height = _labels->height();
        const int width_up = (_labels->width() << 1);
        const int height_up = (_labels->height() << 1);

        const int size = (width * height);
        const int size_up = (width_up * height_up);
        unsigned int i, j, iter;
        int ix, iy, cx, cy, nr_cc = 0;
        float prob, prob_half, prob_quarter;
        OCCArray<T> result;

        /*accumulate prob*/
        std::vector<Pair<float, float> > seeds;
        /*pre-allocate seeds memory*/
        seeds.reserve(300);

        const float *cptr_prob = prob_map[0];
        const float *cptr_offset = offset_map;

        //clear the upsampled seed buffer
        memset((void*)_upsample_seed_map[0], 0, size_up*sizeof(float));

        for (i = 0; i < (unsigned int)height; ++i)
        {
            for (j = 0; j < (unsigned int)width; ++j, ++cptr_prob, cptr_offset += 2)
            {
                prob = cptr_prob[0];

                /*small prob does not contribute*/
                if (prob <= prob_cutoff_thre_contribute)
                {
                    continue; 
                }

                /*upsampling*/
                cy = i_round(((float)i + cptr_offset[0]) * 2.0f);
                cx = i_round(((float)j + cptr_offset[1]) * 2.0f);

                int cym1 = cy - 1;
                int cyp1 = cy + 1;
                int cxm1 = cx - 1;
                int cxp1 = cx + 1;

                if (cx <= 0 || cy <= 0 || cx >= (width_up-1) || cy >= (height_up-1))
                {
                    continue;
                }

                /*assign probs*/
                prob_half = prob * 0.5f;
                prob_quarter = prob_half * 0.5f;

                _upsample_seed_map[cym1][cxm1] += prob_quarter;
                _upsample_seed_map[cym1][cx] += prob_half;
                _upsample_seed_map[cym1][cxp1] += prob_quarter;

                _upsample_seed_map[cy][cxm1] += prob_half;
                _upsample_seed_map[cy][cx] += prob;
                _upsample_seed_map[cy][cxp1] += prob_half;

                _upsample_seed_map[cyp1][cxm1] += prob_quarter;
                _upsample_seed_map[cyp1][cx] += prob_half;
                _upsample_seed_map[cyp1][cxp1] += prob_quarter;
            }
        }

        i_dense_nms_squared_5x5_rot_f(_upsample_seed_map,
            _workarray_flt_up,
            width_up,
            height_up,
            2,
            2,
            width_up - 2,
            height_up - 2,
            prob_cutoff_thre_seed);
 
        for (i = 0; i < (unsigned int)height_up; ++i)
        {
            for (j = 0; j < (unsigned int)width_up; ++j)
            {
                /*valid seed with high confidence*/
                if (_workarray_flt_up[i][j] > 0.f)
                {
                    seeds.push_back(i_make_pair<float, float>((float)j * 0.5f, (float)i * 0.5f));
                }
            }
        }

        if (verbose)
        {
            std::cout << "# of Seeds " << seeds.size() << std::endl;
        }

        /*clear buffer:*/
        memset((void*)_masks->data(0), 0, size*sizeof(unsigned char));
        memset((void*)_workarray_uchar[0], 0, size*sizeof(unsigned char));

        for (i = 0; i < (unsigned int)size; ++i)
        {
            prob = prob_map[0][i];
            if (prob > prob_cutoff_thre_high)
            {
                (_masks->data(0))[i] = 1;
            }
            else
            {
                if (prob > prob_cutoff_thre_low)
                {
                    _workarray_uchar[0][i] = 1;
                }
            }
        }

        /*segment the first-pass: core*/
        nr_cc = binray_cc8_labeling(_masks->const_data(),
            _labels->data(),
            (unsigned int)width,
            (unsigned int)height);

        nr_cc = (*_occs)[0].init(_labels->const_data2(0), width, height);

        if (verbose)
        {
            std::cout << "# of Connected Components (1st pass) " << nr_cc << std::endl;
        }

        /*assign ccs to seeds - first pass*/
        nr_cc = match_ccs_and_seeds((*_occs)[0], offset_map, seeds, 
            result, width, height, cc_to_center_dist_thre, 0);

        /*max number of ccs must be smaller than max of unsigned short*/
        assert(result.size() == seeds.size());
        assert(result.size() < (unsigned int)Constant<unsigned short>::MAX_VAL());

        for (iter = 0; iter < nr_iter; ++iter)
        {
            /*background label*/
            memset((void*)_workarray_uint[0], 0, size*sizeof(unsigned int)); 

            for (i = 0; i < result.size(); ++i)
            {
                for (j = 0; j < result[i].size(); ++j)
                {
                    ix = i_round(result[i].x(j));
                    iy = i_round(result[i].y(j));
                    assert(ix < width  && ix >= 0);
                    assert(iy < height && iy >= 0);
                    /*label id = i + 1*/
                    _workarray_uint[iy][ix] = (i + 1);
                }
            }

            propagate_neighbor_cc((const unsigned int**)_workarray_uint,
                result,
                _workarray_uchar,
                width,
                height);
        }

        nr_cc = match_node_and_seeds(_workarray_uchar, offset_map, seeds,
            result, width, height, cc_to_center_dist_thre);

        /*remove empty ccs (seeds that with no match)*/
        (*_occs)[0].clear();

        for (i = 0; i < result.size(); ++i)
        {
            if (!result[i].empty())
            {
                (*_occs)[0].push_back(result[i]);
            }
        }

        if (verbose)
        {
            std::cout << "scale: " << 0 << ", # of cc (processed): " 
                << (*_occs)[0].size() << std::endl;
        }

        /*save results*/
        if (labels != NULL)
        {
            /*max number of ccs must be smaller than max of unsigned short*/
            assert((*_occs)[0].size() < (unsigned int)Constant<unsigned short>::MAX_VAL());
            memset((void*)labels, 0, size*sizeof(unsigned short));

            for (i = 0; i < (*_occs)[0].size(); ++i)
            {
                for (j = 0; j < (*_occs)[0][i].size(); ++j)
                {
                    ix = i_round((*_occs)[0][i].x(j));
                    iy = i_round((*_occs)[0][i].y(j));
                    assert(ix < width && ix >= 0);
                    assert(iy < height && iy >= 0);
                    labels[iy*width + ix] = (unsigned short)(i + 1);
                }
            }
        }

        /*note that rendering the canvas is slow*/
        if (canvas != NULL)
        {
            srand((unsigned int)time(NULL)); 
            /*generate a random seed*/
            int rseed = rand();

            memset((void*)canvas, 0, size * 3 * sizeof(unsigned char));

            int clr[3], max_clr;

            for (i = 0; i < (*_occs)[0].size(); ++i)
            {
                clr[0] = i_rand_i(256, rseed);
                clr[1] = i_rand_i(256, rseed);
                clr[2] = i_rand_i(256, rseed);
                max_clr = i_max(i_max(clr[0], clr[1]), clr[2]);

                /*avoid dark color*/
                if (max_clr < 64)
                {
                    clr[i % 3] = 128;
                }

                for (j = 0; j < (*_occs)[0][i].size(); ++j)
                {
                    int x = (int)(*_occs)[0][i].x(j);
                    int y = (int)(*_occs)[0][i].y(j);
                    canvas[(y * width + x) * 3] = (unsigned char)clr[0];
                    canvas[(y * width + x) * 3 + 1] = (unsigned char)clr[1];
                    canvas[(y * width + x) * 3 + 2] = (unsigned char)clr[2];
                }
            }
        }
        return (int)(((*_occs)[0]).size());
    }

}/*namespace idl*/

#endif